#include <message_filters/subscriber.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <geometry_msgs/Vector3Stamped.h>
#include "opencv2/gpu/gpu.hpp"
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <Eigen/Eigen>
#include <opencv2/opencv.hpp>
#include <message_filters/sync_policies/approximate_time.h>


using namespace std;

bool colourInRange(unsigned char    red,
                   unsigned char    green,
                   unsigned char    blue,
                   Eigen::Vector3f  range)
{
    // CALCULATES IF RGB VALUE OF PIXEL IS IN SPECIFIED RANGE
    //
    // INPUT:   RGB-VALUE OF PIXEL AND RANGEhist_test
    // OUTPUT:  TRUE OR FALSE

    const char diffRange = 50;
    if( (red >= range[0] - diffRange && red <= range[0] + diffRange)        &&
        (green >= range[1] - diffRange && green <= range[1] + diffRange)    &&
        (blue >= range[2] - diffRange && blue <= range[2] + diffRange)    )
    {
        return true;
    }

    return false;
}


void callback(const sensor_msgs::ImageConstPtr& image, const geometry_msgs::Vector3StampedConstPtr& colour_info)
{
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

    //ROS_ERROR("START");
    //std::cout << image->header.seq << std::endl;
    //std::cout << colour_info->header.seq << std::endl;
    cv::Mat src;// = new cv::Mat(cv::Mat::zeros(cv_ptr->image.rows, cv_ptr->image.rows, CV_8UC3));
    cv::Mat dst;// = new cv::Mat(cv::Mat::zeros(cv_ptr->image.rows, cv_ptr->image.rows, CV_8UC3));
    //cv::Mat srct;

    // Convert FUERTE kinect image to ELECTRIC OpenCV image
    cv::cvtColor(cv_ptr->image, src, CV_BGR2RGB);
    //cv::dilate(src, src,cv::Mat(),cv::Point(-1,-1),2);
    //cv::erode(src, src,cv::Mat(),cv::Point(-1,-1),2);
    cv::imshow("in", src);

    Eigen::Vector3f floor_range;
    floor_range[0] = colour_info->vector.x;
    floor_range[1] = colour_info->vector.y;
    floor_range[2] = colour_info->vector.z;



    for (int i = 0; i < src.rows; i++)
    {
        if(i >= src.rows*2/3)
        {
            for (int j = 0; j < src.cols; j += 1)
            {
                unsigned char red = src.at<cv::Vec3b>(i,j)[0];
                unsigned char green = src.at<cv::Vec3b>(i,j)[1];
                unsigned char blue = src.at<cv::Vec3b>(i,j)[2];

                if (!colourInRange(red, green, blue, floor_range))
                {
                    //const unsigned char value = 0;
                    src.at<cv::Vec3b>(i,j)[2] = blue;
                    src.at<cv::Vec3b>(i,j)[1] = green;
                    src.at<cv::Vec3b>(i,j)[0] = red;
                }
                else
                {
                    const unsigned char value = 255;
                    src.at<cv::Vec3b>(i,j)[2] = value;
                    src.at<cv::Vec3b>(i,j)[1] = value;
                    src.at<cv::Vec3b>(i,j)[0] = value;
                }
            }
        }
    }

//    vector<vector<Point> > contours;
//    vector<Vec4i> hierarchy;
//    cv::findContours(graySegmentation, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE);
//    Mat drawing = Mat::zeros( graySegmentation.size(), CV_8UC3 );
//    for (size_t i = 0; i < contours.size(); ++i)
//        {
//        Mat m(contours[i]);
//        double area = contourArea(m);
//        CvScalar color = CV_RGB(255, 255, 255);
//        if(area> 200){
//            drawContours( drawing, contours, i, color, CV_FILLED, 8, hierarchy, -1, Point() ) ;
//        }
//    }

    cv::imshow("test", src);
    cv::waitKey(3);
    //cv::imshow("WINDOW", *dst);
    //ROS_ERROR("END");
    //delete *dst;
    //delete *src;


}

int main(int argc, char** argv)
{
  cv::namedWindow("test", CV_WINDOW_NORMAL );
  cv::namedWindow("in", CV_WINDOW_NORMAL );
  ros::init(argc, argv, "godot_floor_segmentation");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);

  image_transport::SubscriberFilter image_sub(it, "/camera/rgb/image_color", 1);
  message_filters::Subscriber<geometry_msgs::Vector3Stamped> colour_sub(nh, "godot/colour_floor", 1);

  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, geometry_msgs::Vector3Stamped> MySyncPolicy;
  message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_sub, colour_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}
